//
// Created by lqk on 2022/1/24.
//

#include "GazeboMotorsDriver.h"




void GazeboMotorsDriver::Init() {
    m_rbc.addClient("topic_advertiser");
    m_rbc.advertise("topic_advertiser", "/quad_bot/all_joints_effort_controller/command", "std_msgs/String");

    m_rbc.addClient("topic_subscriber");
    m_rbc.subscribe("topic_subscriber", "/quad_bot/joint_states", GazeboMotorsDriver::SubscribeCallback);


}
void GazeboMotorsDriver::RunOnce() {
    //    ActuatorSimulate();
    SendCommand();

}

void GazeboMotorsDriver::ActuatorSimulate() {
    double torqueAbad[4],torqueHip[4],torqueKnee[4];
    // * Test
    for(int i=0;i<4;i++){
        g_motorsCommand.kpAbad[i]=1;
        g_motorsCommand.kpHip[i]=1;
        g_motorsCommand.kpKnee[i]=1;
        g_motorsCommand.kdAbad[i]=.3;
        g_motorsCommand.kdHip[i]=.3;
        g_motorsCommand.kdKnee[i]=.3;
    }
    for(int i=0;i<4;i++){
        torqueAbad[i]=g_motorsCommand.kpAbad[i]*(g_motorsCommand.qDesAbad[i] - g_motorsInfo.posAbad[i]) + g_motorsCommand.kdAbad[i] * (g_motorsCommand.qdDesAbad[i] - g_motorsInfo.velAbad[i]) + g_motorsCommand.tauAbadFF[i];
        torqueHip[i]=g_motorsCommand.kpHip[i]*(g_motorsCommand.qDesHip[i] - g_motorsInfo.posHip[i]) + g_motorsCommand.kdHip[i] * (g_motorsCommand.qdDesHip[i] - g_motorsInfo.velHip[i]) + g_motorsCommand.tauHipFF[i];
        torqueKnee[i]=g_motorsCommand.kpKnee[i]*(g_motorsCommand.qDesKnee[i] - g_motorsInfo.posKnee[i]) + g_motorsCommand.kdKnee[i] * (g_motorsCommand.qdDesKnee[i] - g_motorsInfo.velKnee[i]) + g_motorsCommand.tauKneeFF[i];
        //TODO Set torque limits

        motorsTorque[3*i]=torqueAbad[i];
        motorsTorque[3*i+1]=torqueHip[i];
        motorsTorque[3*i+2]=torqueKnee[i];
    }

}

void GazeboMotorsDriver::SendCommand() {
    m_rbc.addClient("topic_publisher");
// * Test
//    for(int i=0;i<12;i++){
////        motorsTorque[i]=(float)1;
////       std::cout<<" "<<motorsTorque[i];
//    }
//    //Construct std_msgs/Float64MultiArray message
//    rapidjson::Document dim;
//    dim.SetObject();
//    dim.AddMember("label","Efforts",dim.GetAllocator());
//    dim.AddMember("size",12,dim.GetAllocator());
//    dim.AddMember("stride",12,dim.GetAllocator());
//    rapidjson::Document layout;
//    rapidjson::Value dimsValue(rapidjson::kArrayType);
//    dimsValue.PushBack(dim,layout.GetAllocator());
//    layout.SetObject();
//    layout.AddMember("dim",dimsValue,layout.GetAllocator());
//    layout.AddMember("data_offset",0,layout.GetAllocator());
//    rapidjson::Document msg;
//    msg.SetObject();
//    rapidjson::Value efforts(rapidjson::kArrayType);
//    for(int i=0;i<12;i++){
//        efforts.PushBack(motorsTorque[i],msg.GetAllocator());
//    }
//    msg.AddMember("layout",layout,msg.GetAllocator());
//    msg.AddMember("data",efforts,msg.GetAllocator());
//    std::cout<<" Task: publish motor torques"<<std::endl;
    number++;
    if(number>500) number=0;
    std::cout<<number<<std::endl;
    rapidjson::Document d;
    d.SetObject();
    d.AddMember("data", "Test message", d.GetAllocator());

}

void GazeboMotorsDriver::SubscribeCallback(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage> in_message) {
//    std::string msg;
//    std::vector<float> pos,vel;
//    msg=in_message->string();
//
//    rapidjson::Document doc;
//    doc.Parse(msg.c_str());
//    assert(doc["msg"].HasMember("position"));
//    for(auto& p:doc["msg"]["position"].GetArray()){
////        std::cout<<p.GetFloat()<<" ";
//        pos.push_back(p.GetFloat());
//    }
//    for(auto& v:doc["msg"]["velocity"].GetArray()){
//        //std::cout<<v.GetFloat()<<" ";
//        vel.push_back(v.GetFloat());
//    }
//    g_motorsInfo.posAbad[0]=pos[9];
//    g_motorsInfo.posHip[0]=pos[1];
//    g_motorsInfo.posKnee[0]=pos[5];
//    g_motorsInfo.posAbad[1]=pos[8];
//    g_motorsInfo.posHip[1]=pos[0];
//    g_motorsInfo.posKnee[1]=pos[4];
//    g_motorsInfo.posAbad[2]=pos[11];
//    g_motorsInfo.posHip[2]=pos[3];
//    g_motorsInfo.posKnee[2]=pos[7];
//    g_motorsInfo.posAbad[3]=pos[10];
//    g_motorsInfo.posHip[3]=pos[2];
//    g_motorsInfo.posKnee[3]=pos[6];
//
//    g_motorsInfo.velAbad[0]=vel[9];
//    g_motorsInfo.velHip[0]=vel[1];
//    g_motorsInfo.velKnee[0]=vel[5];
//    g_motorsInfo.velAbad[1]=vel[8];
//    g_motorsInfo.velHip[1]=vel[0];
//    g_motorsInfo.velKnee[1]=vel[4];
//    g_motorsInfo.velAbad[2]=vel[11];
//    g_motorsInfo.velHip[2]=vel[3];
//    g_motorsInfo.velKnee[2]=vel[7];
//    g_motorsInfo.velAbad[3]=vel[10];
//    g_motorsInfo.velHip[3]=vel[2];
//    g_motorsInfo.velKnee[3]=vel[6];
//    std::cout << "subscriberCallback(): Message Received: " << msg << std::endl;

}

void GazeboMotorsDriver::CleanUp() {
    m_rbc.removeClient("topic_advertiser");
    m_rbc.removeClient("topic_subscriber");
}